﻿using Agvlaser.map;
using Agvlaser.Sqlite;
using Newtonsoft.Json;
using RosSharp.RosBridgeClient;
using System;
using System.Collections.Generic;
using System.Data.SQLite;
using System.Drawing;
using System.Threading;
using System.Windows;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Shapes;
using static Agvlaser.BindHelper;
using static Agvlaser.DataStructClass;

namespace Agvlaser
{
    /// <summary>
    /// MainWindow.xaml 的交互逻辑
    /// </summary>
    public partial class MainWindow : Window
    {
        #region 绑定元素初始化
        //底部消息栏
        BottomInfoBound bottomInfo = new BottomInfoBound();
        public void BottomInfoInit()
        {
            bottomInfo.RobotAngle = "当前角度: 98";
            bottomInfo.ViewScale = string.Format("视图缩放: {0}", RosHelper.viewScale);
            bottomInfo.OriginPoint = string.Format("原点坐标：X:{0},Y:{1}", RosHelper.originPoint.X, RosHelper.originPoint.Y);
            bottomInfo.WordAxis = "世界坐标: X:3.47,Y:6.82";
            bottomInfo.ViewAxis = "视图坐标: X:13.75,Y:9.23";

            //整个窗口内的所有元素都可以绑定此数据
            //this.DataContext = bottomLabel;

            //仅stackPanel内的所有元素可以绑定此数据
            bottomstackPanel.DataContext = bottomInfo;
        }
        private void UpdateBottomInfo()
        {
            bottomInfo.ViewScale = string.Format("视图缩放: {0}", RosHelper.viewScale);
            bottomInfo.OriginPoint = string.Format("原点坐标：X:{0},Y:{1}", RosHelper.originPoint.X, RosHelper.originPoint.Y);
        }
        RouteEditBound routeEdit = new RouteEditBound();
        public void RouteEditInit()
        {
            SqliteHelper sql = new SqliteHelper();
            //从数据库读取当前路径路径信息
            string str = sql.ExecuteScalar(string.Format("select route from route where num={0}", GlobalVar.systemConfig.RouteConfig.RouteNow)) as string;
            //存在显示路径信息
            if (str != null)
            {
                //转换为对象
                GlobalVar.routeInfo = JsonConvert.DeserializeObject<Route>(str);
                routeEdit.RouteNow = string.Format("当前路径：{0}", GlobalVar.systemConfig.RouteConfig.RouteNow);
                routeEdit.RouteDes = string.Format("描述信息：{0}", GlobalVar.routeInfo.RouteDes);
                routeEdit.StationTotal = string.Format("包含站点：{0}", GlobalVar.routeInfo.StationTotal);
                if (GlobalVar.routeInfo.StationTotal > 0)
                {
                    routeEdit.StationNow = string.Format("当前站点：{0}", GlobalVar.systemConfig.RouteConfig.StationNow);
                    routeEdit.StationInfoX = string.Format("X-坐标：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.x);
                    routeEdit.StationInfoY = string.Format("Y-坐标：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.y);
                    routeEdit.StationInfoAngel = string.Format("角  度：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].StationInfoAngel);
                    routeEdit.StationInfoStopTime = string.Format("停留时间：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].StationInfoStopTime);
                    routeEdit.StationDes = string.Format("站点描述：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].StationDes);
                }
                else
                {
                    routeEdit.StationNow = string.Format("当前站点：{0}", "");
                    routeEdit.StationInfoX = string.Format("X-坐标：{0}", "");
                    routeEdit.StationInfoY = string.Format("Y-坐标：{0}", "");
                    routeEdit.StationInfoAngel = string.Format("角  度：{0}", "");
                    routeEdit.StationInfoStopTime = string.Format("停留时间：{0}", "");
                    routeEdit.StationDes = string.Format("站点描述：{0}", "");
                }

                //禁止按键操作
                btnAddStation.IsEnabled = true;
                btnDeleteStation.IsEnabled = true;
                btnEditStation.IsEnabled = true;
                btnRunStation.IsEnabled = true;
                btnNextStation.IsEnabled = true;
                btnLastStation.IsEnabled = true;
            }
            //不存在清空显示
            else
            {
                routeEdit.RouteNow = string.Format("当前路径：{0}", "无");
                routeEdit.RouteDes = string.Format("描述信息：{0}", "未创建，请先创建路径添！");
                routeEdit.StationTotal = string.Format("包含站点：{0}", "");
                routeEdit.StationNow = string.Format("当前站点：{0}", "");
                routeEdit.StationInfoX = string.Format("X-坐标：{0}", "");
                routeEdit.StationInfoY = string.Format("Y-坐标：{0}", "");
                routeEdit.StationInfoAngel = string.Format("角  度：{0}", "");
                routeEdit.StationInfoStopTime = string.Format("停留时间：{0}", "");
                routeEdit.StationDes = string.Format("站点描述：{0}", "");

                //禁止按键操作
                btnAddStation.IsEnabled = false;
                btnDeleteStation.IsEnabled = false;
                btnEditStation.IsEnabled = false;
                btnRunStation.IsEnabled = false;
                btnNextStation.IsEnabled = false;
                btnLastStation.IsEnabled = false;
            }
            routeEditStackPanel.DataContext = routeEdit;
        }
        /// <summary>
        /// 更新路径信息显示
        /// </summary>
        public void RouteInfoUpdate()
        {
            routeEdit.RouteNow = string.Format("当前路径：{0}", GlobalVar.systemConfig.RouteConfig.RouteNow);
            routeEdit.RouteDes = string.Format("描述信息：{0}", GlobalVar.routeInfo.RouteDes);
            routeEdit.StationTotal = string.Format("包含站点：{0}", GlobalVar.routeInfo.StationTotal);
            if (GlobalVar.routeInfo.StationTotal > 0)
            {
                routeEdit.StationNow = string.Format("当前站点：{0}", GlobalVar.systemConfig.RouteConfig.StationNow);
                routeEdit.StationInfoX = string.Format("X-坐标：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.x);
                routeEdit.StationInfoY = string.Format("Y-坐标：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.y);
                routeEdit.StationInfoAngel = string.Format("角  度：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].StationInfoAngel);
                routeEdit.StationInfoStopTime = string.Format("停留时间：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].StationInfoStopTime);
                routeEdit.StationDes = string.Format("站点描述：{0}", GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].StationDes);
            }
        }
        #endregion

        #region Windows
        public MainWindow()
        {
            InitializeComponent();
        }
        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            SqliteHelper.SetConnectionString("sqlite.db3", "");
            SqliteHelper sql = new SqliteHelper();
            // 获取本地地图
            string str = sql.ExecuteScalar("select value from map where key='test' limit 1") as string;
            GlobalVar.mapGrid = JsonConvert.DeserializeObject<NavigationOccupancyGrid>(str);

            #region 初始化系统配置
            str = sql.ExecuteScalar("select value from config where key='system' limit 1") as string;
            //不存在，初始化配置，在数据库增加记录
            if (str == null)
            {
                GlobalVar.systemConfig = new SystemConfig();
                GlobalVar.systemConfig.RouteConfig.RouteNow = 0;
                GlobalVar.systemConfig.RouteConfig.StationNow = 0;
                GlobalVar.systemConfig.RobotIp = "192.168.1.125";
                //配置对象转换为json串
                str = JsonConvert.SerializeObject(GlobalVar.systemConfig, Formatting.Indented);
                //保存配置
                int res = sql.ExecuteNonQuery(string.Format("insert into config (key,value) VALUES ('{0}','{1}')", "system", str));
                if (res <= 0)
                    MessageBox.Show("初始化系统参数失败，请检查数据库！");
            }
            else
            //存在，转换为系统配置对象
            {
                GlobalVar.systemConfig = JsonConvert.DeserializeObject<SystemConfig>(str);
            }
            #endregion

            #region 初始化绘图
            //地图尺寸
            GlobalVar.MapSize = new System.Drawing.Size((int)GlobalVar.mapGrid.info.width, (int)GlobalVar.mapGrid.info.height);
            //绘图区尺寸
            RosHelper.viewSize.Width = (int)gridDraw.ActualWidth;
            RosHelper.viewSize.Height = (int)gridDraw.ActualHeight;
            //搜索地图有效区域
            RosHelper.validArea = RosHelper.SearchEffectiveArea(GlobalVar.mapGrid);
            //获取地图障碍物坐标集合
            RosHelper.mapObstaclePoints = RosHelper.GetObstacleAxis(GlobalVar.mapGrid);
            #endregion

            #region 初始化绑定元素
            //底部信息栏
            BottomInfoInit();
            //路径编辑栏
            RouteEditInit();
            #endregion

            //显示区域复位
            RestView();
        }
        //关闭系统前关闭已开启进程
        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            if (GlobalVar.rosSocket != null)
            {
                GlobalVar.rosSocket.Close();
            }
        }

        #endregion

        //创建监听和发布进程功能函数
        private void listen()
        {
            GlobalVar.TocpicList.Clear();
            //GlobalVar.rosSocket = new RosSocket("ws://172.20.10.2:9090");
            //GlobalVar.rosSocket = new RosSocket("ws://192.168.149.133:9090");
            GlobalVar.rosSocket = new RosSocket("ws://192.168.1.125:9090");
            //GlobalVar.rosSocket = new RosSocket("ws://192.168.47.128:9090");

            string subid;

            //订阅定位坐标
            subid = GlobalVar.rosSocket.Subscribe("/amcl_pose", "geometry_msgs/PoseWithCovarianceStamped", subHandlerAmclPose);
            GlobalVar.TocpicList.Add("amcl_pose", subid);

            //订阅全局路径
            //subid = GlobalVar.rosSocket.Subscribe("/move_base/NavfnROS/plan", MessageList.nav_msgs.Path, subHandlerGlobalPlan);
            subid = GlobalVar.rosSocket.Subscribe("/move_base/GlobalPlanner/plan", MessageList.nav_msgs.Path, subHandlerGlobalPlan);
            GlobalVar.TocpicList.Add("global_plan", subid);

            //订阅局部路径
            subid = GlobalVar.rosSocket.Subscribe("/move_base/TrajectoryPlannerROS/local_plan", MessageList.nav_msgs.Path, subHandlerLocalPlan);
            GlobalVar.TocpicList.Add("local_plan", subid);

            //订阅TF
            //subid = GlobalVar.rosSocket.Subscribe("/tf", MessageList.tf2_msgs.TFMessage, subHandlerTF);
            //GlobalVar.TocpicList.Add("tf", subid);

            //订阅MoveBaseActionResult
            subid = GlobalVar.rosSocket.Subscribe("/move_base/result", MessageList.move_base_msgs.MoveBaseActionResult, subHandlerMoveBaseActionResult);
            GlobalVar.TocpicList.Add("MoveBaseActionResult", subid);

            //发布机器人控制话题
            subid = GlobalVar.rosSocket.Advertise("/cmd_vel", MessageList.geometry_msgs.Twist);
            GlobalVar.TocpicList.Add("cmd_vel", subid);
        }

        //消息输出框
        private void msg(string msg, System.Windows.Media.Brush brushe)
        {
            string str = DateTime.Now.ToString("yyyy年MM月dd日 HH:mm:ss:ms");
            richboxMsg.Document.Blocks.Add(new Paragraph(new Run(str) { Foreground = System.Windows.Media.Brushes.Orange }));
            richboxMsg.Document.Blocks.Add(new Paragraph(new Run(msg) { Foreground = brushe }));
            richboxMsg.ScrollToEnd();
        }
        private void msg(string msg)
        {
            string str = DateTime.Now.ToString("yyyy年MM月dd日 HH:mm:ss:ms");
            richboxMsg.Document.Blocks.Add(new Paragraph(new Run(str) { Foreground = System.Windows.Media.Brushes.Orange }));
            richboxMsg.Document.Blocks.Add(new Paragraph(new Run(msg) { Foreground = System.Windows.Media.Brushes.Green }));
            richboxMsg.ScrollToEnd();
        }
        #region 导航栏
        //连接机器人
        private void btnConnect_Click(object sender, RoutedEventArgs e)
        {
            if (GlobalVar.Robot.Connected == true)
            {
                GlobalVar.Robot.Connected = false;
                btnConnect.Content = "连接机器人";
                GlobalVar.rosSocket.Close();
                //Thread thread = new Thread(listen);
                //thread.IsBackground = true;
                //thread.Start();
            }
            else
            {
                GlobalVar.Robot.Connected = true;
                btnConnect.Content = "断开连接";
                listen();
            }

        }
        //手动加载地图
        private void btnLoadMap_Click(object sender, RoutedEventArgs e)
        {
            //订阅地图话题
            string str = GlobalVar.rosSocket.Subscribe("/map", MessageList.nav_msgs.OccupancyGrid, subHandlerMap);
            GlobalVar.TocpicList.Add("map", str);

            GlobalVar.TocpicList.TryGetValue("map", out str);
        }
        //重置机器人位置
        private void btnResetRobot_Click(object sender, RoutedEventArgs e)
        {
            GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.ResetRobot;
        }
        //手动发布位置
        private void btnPublishPose_Click(object sender, RoutedEventArgs e)
        {
            GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.SetTargetPose;
        }
        //发布上一个存储的坐标
        private void btn_AutoGo_Click(object sender, RoutedEventArgs e)
        {
            //发布目标位置话题
            string publication__id_Go = GlobalVar.rosSocket.Advertise("/move_base_simple/goal", MessageList.geometry_msgs.PoseStamped);

            GeometryPoseStamped gps = new GeometryPoseStamped();
            gps.pose.position.x = LastLocation.pose.pose.position.x;
            gps.pose.position.y = LastLocation.pose.pose.position.y;

            gps.pose.orientation.z = LastLocation.pose.pose.orientation.z;
            gps.pose.orientation.w = LastLocation.pose.pose.orientation.w;
            gps.header = LastLocation.header;
            GlobalVar.rosSocket.Publish(publication__id_Go, gps);

            msg(string.Format(
                "发布了一个坐标:\r\n" +
                "X:{0}\r\n " +
                "Y:{1}\r\n " +
                "Z:{2}",
                LastLocation.pose.pose.position.x,
                LastLocation.pose.pose.position.y,
                LastLocation.pose.pose.orientation.z), System.Windows.Media.Brushes.Red);
        }
        //存储当前坐标
        GeometryPoseWithCovarianceStamped LastLocation;
        private void btn_SaveLocation_Click(object sender, RoutedEventArgs e)
        {
            LastLocation = AmclPoseStamped;
            msg(string.Format(
                "存贮了一个位置：\r\n " +
                "X:{0}\r\n " +
                "Y:{1}\r\n " +
                "Z:{2}",
                AmclPoseStamped.pose.pose.position.x,
                AmclPoseStamped.pose.pose.position.y,
                AmclPoseStamped.pose.pose.orientation.z), System.Windows.Media.Brushes.Green);
        }
        //退出系统
        private void btn_Exit_Click(object sender, RoutedEventArgs e)
        {
            MessageBoxResult dr = MessageBox.Show("确定要退出吗?", "提示", MessageBoxButton.OKCancel, MessageBoxImage.Question);
            if (dr == MessageBoxResult.OK)
            {
                this.Close();
            }
        }
        //重置地图视图
        private void btnResetView_Click(object sender, RoutedEventArgs e)
        {
            RestView();
        }
        #endregion

        #region 订阅回调
        //订阅局部路径回调
        NavigationPath localPath;
        private void subHandlerLocalPlan(Message message)
        {
            localPath = (NavigationPath)message;
            DrawgLocalPlan();
        }
        NavigationPath globalPath;
        //订阅全局路径回调
        private void subHandlerGlobalPlan(Message message)
        {
            globalPath = (NavigationPath)message;

            DrawGlobalPlan();
        }
        //订阅地图回调
        private void subHandlerMap(Message message)
        {
            //接收地图数据
            GlobalVar.mapGrid = (NavigationOccupancyGrid)message;
            string str = JsonConvert.SerializeObject(GlobalVar.mapGrid, Formatting.Indented);
            //更新地图尺寸
            GlobalVar.MapSize = new System.Drawing.Size((int)GlobalVar.mapGrid.info.width, (int)GlobalVar.mapGrid.info.height);
            //搜索地图有效区域
            RosHelper.validArea = RosHelper.SearchEffectiveArea(GlobalVar.mapGrid);
            //获取地图障碍物坐标集合
            RosHelper.mapObstaclePoints = RosHelper.GetObstacleAxis(GlobalVar.mapGrid);

            //存储地图到数据库
            SQLiteConnection m_dbConnection = new SQLiteConnection("Data Source=sqlite.db3;Version=3;");
            m_dbConnection.Open();
            string sql = string.Format("update map set value = '{0}' where key = 'test'", str);
            SQLiteCommand command = new SQLiteCommand(sql, m_dbConnection);
            command.ExecuteNonQuery();
            m_dbConnection.Close();

            //取消订阅
            string sub_id;
            GlobalVar.TocpicList.TryGetValue("map", out sub_id);
            GlobalVar.rosSocket.Unsubscribe(sub_id);

            //更新显示
            DrawMap();
        }
        //订阅Amcl定位坐标回调
        GeometryPoseWithCovarianceStamped AmclPoseStamped = new GeometryPoseWithCovarianceStamped();
        private void subHandlerAmclPose(Message message)
        {
            GeometryPoseWithCovarianceStamped Amcl = (GeometryPoseWithCovarianceStamped)message;
            //有变化才重新绘制
            if (!AmclPoseStamped.pose.pose.Equals(Amcl.pose.pose))
            {
                this.Dispatcher.Invoke(new Action(delegate
                {
                    DrawRobot();
                }));
            }
            AmclPoseStamped = Amcl;
            this.Dispatcher.Invoke(new Action(delegate
            {
                // string str = JsonConvert.SerializeObject(AmclPoseStamped, Formatting.Indented);
                string str = string.Format("\r\n" +
                "  seq:{0}\r\n" +
                "  nsecs:{8}\r\n" +
                "  Position:\r\n" +
                "    x:{1}\r\n" +
                "    y:{2}\r\n" +
                "    z:{3}\r\n" +
                "  Orientation:\r\n" +
                "    x:{4}\r\n" +
                "    y:{5}\r\n" +
                "    z:{6}\r\n" +
                "    w:{7}\r\n",
                AmclPoseStamped.header.stamp.secs,
                AmclPoseStamped.pose.pose.position.x,
                AmclPoseStamped.pose.pose.position.y,
                AmclPoseStamped.pose.pose.position.z,
                AmclPoseStamped.pose.pose.orientation.x,
                AmclPoseStamped.pose.pose.orientation.y,
                AmclPoseStamped.pose.pose.orientation.z,
                AmclPoseStamped.pose.pose.orientation.w,
                AmclPoseStamped.header.stamp.nsecs
                );
                richboxPose.Document.Blocks.Clear();
                richboxPose.Document.Blocks.Add(new Paragraph(new Run(str) { Foreground = System.Windows.Media.Brushes.YellowGreen }));
                richboxPose.ScrollToEnd();

                //更新底部坐标信息
                bottomInfo.WordAxis = string.Format("世界坐标: X:{0}, Y: {1}", Math.Round(AmclPoseStamped.pose.pose.position.x, 2), Math.Round(AmclPoseStamped.pose.pose.position.y, 2));
            }));
        }
        //订阅TF回调
        GeometryTransFormStamped baselink;
        private void subHandlerTF(Message message)
        {
            Tf2TFMessage tf = (Tf2TFMessage)message;
            for (int i = 0; i < tf.transforms.Length; i++)
            {
                if (tf.transforms[i].child_frame_id == "base_link")
                {
                    baselink = tf.transforms[i];
                    this.Dispatcher.Invoke(new Action(delegate
                    {
                        string str = string.Format("\r\n" +
                            "  seq:{0}\r\n" +
                            "  Position:\r\n" +
                            "    x:{1}\r\n" +
                            "    y:{2}\r\n" +
                            "    z:{3}\r\n" +
                            "  Orientation:\r\n" +
                            "    x:{4}\r\n" +
                            "    y:{5}\r\n" +
                            "    z:{6}\r\n" +
                            "    w:{7}\r\n",
                            baselink.header.stamp.secs,
                            baselink.transform.translation.x,
                            baselink.transform.translation.y,
                            baselink.transform.translation.z,
                            baselink.transform.rotation.x,
                            baselink.transform.rotation.y,
                            baselink.transform.rotation.z,
                            baselink.transform.rotation.w
                            );
                        richboxPose.Document.Blocks.Clear();
                        richboxPose.Document.Blocks.Add(new Paragraph(new Run(str) { Foreground = System.Windows.Media.Brushes.YellowGreen }));
                        richboxPose.ScrollToEnd();
                        // DrawRobot();

                        //更新底部坐标信息
                        //bottomInfo.WordAxis = string.Format("世界坐标: X:{0}, Y: {1}", Math.Round(baselink.transform.translation.x, 2), Math.Round(baselink.transform.translation.y, 2));
                    }));
                }
            }
        }
        //订阅MoveBaseActionResult回调
        MoveBaseActionResult moveBaseActionResult = new MoveBaseActionResult();
        private void subHandlerMoveBaseActionResult(Message message)
        {
            moveBaseActionResult = (MoveBaseActionResult)message;
            this.Dispatcher.Invoke(new Action(delegate
            {
                msg(moveBaseActionResult.status.text);
            }));
        }
        #endregion

        #region 绘制界面
        private void DrawMap()
        {
            Bitmap im = new Bitmap(RosHelper.viewSize.Width, RosHelper.viewSize.Height);
            Graphics g = Graphics.FromImage(im);

            //绘制有效区域边框
            //g.DrawRectangle(new Pen(Color.Blue, 1), RosHelper.AxisToView(RosHelper.validArea));

            //绘制无效区域
            //System.Drawing.Rectangle rectangle = RosHelper.AxisToView(0, 0, GlobalVar.MapSize.Width, GlobalVar.MapSize.Height);
            //g.DrawRectangle(new Pen(Color.Orange, 1), rectangle);

            //转换障碍物坐标
            System.Drawing.Rectangle[] mapObstacle = new System.Drawing.Rectangle[RosHelper.mapObstaclePoints.Length];
            for (int i = 0; i < mapObstacle.Length; i++)
            {
                mapObstacle[i] = RosHelper.AxisToView(RosHelper.mapObstaclePoints[i].X, RosHelper.mapObstaclePoints[i].Y, 2, 2);
            }

            //绘制地图障碍物
            g.FillRectangles(Brushes.Gray, mapObstacle);
            this.Dispatcher.Invoke(new Action(delegate
            {
                System.Windows.Controls.Image image = new System.Windows.Controls.Image();
                image.Source = MapHlper.ChangeBitmapToImageSource(im);
                image.Margin = new Thickness(0, 0, 0, 0);

                cvMap.Children.Clear();
                cvMap.Children.Add(image);
            }));

        }
        //绘制机器人
        private void DrawRobot()
        {
            if (AmclPoseStamped == null)
                return;

            //清空绘图区
            cvRobot.Children.Clear();
            //机器人半径
            float Radius = 5 * RosHelper.viewScale;

            //机器人世界坐标
            float x = AmclPoseStamped.pose.pose.position.x;
            float y = AmclPoseStamped.pose.pose.position.y;
            //世界坐标系到视图坐标系
            System.Windows.Point pointf = RosHelper.WorldToView(new System.Windows.Point(x, y));

            //绘制机器人
            var el = new Ellipse();
            el.Height = Radius * 2;
            el.Width = Radius * 2;
            el.Margin = new Thickness(pointf.X - Radius, pointf.Y - Radius, 0, 0);
            el.StrokeThickness = 2;
            el.Fill = System.Windows.Media.Brushes.Yellow;
            el.Stroke = System.Windows.Media.Brushes.Gray;
            cvRobot.Children.Add(el);

            //计算角度
            float angle = (float)(Math.Asin(AmclPoseStamped.pose.pose.orientation.z) * 2 * 180 / Math.PI);
            if (angle <= 0)
            {
                angle += 360;
            }
            float distance = 5 * RosHelper.viewScale;

            //计算坐标
            System.Windows.Point pointEnd = new System.Windows.Point();
            pointEnd.X = pointf.X + (float)(Math.Cos(Math.PI * (angle / 180.0)) * distance);
            pointEnd.Y = pointf.Y - (float)(Math.Sin(Math.PI * (angle / 180.0)) * distance);

            //画方向线
            Line line = new Line();
            line.X1 = pointf.X;
            line.X2 = pointEnd.X;
            line.Y1 = pointf.Y;
            line.Y2 = pointEnd.Y;
            line.StrokeThickness = 1f * RosHelper.viewScale;
            line.Stroke = System.Windows.Media.Brushes.Black;
            cvRobot.Children.Add(line);
        }
        //绘制全局规划路径
        private void DrawGlobalPlan()
        {
            if (globalPath == null)
                return;
            int width = RosHelper.viewSize.Width;
            int height = RosHelper.viewSize.Height;
            Bitmap im = new Bitmap(width, height);
            Graphics g = Graphics.FromImage(im);
            PointF[] pointFs = new PointF[globalPath.poses.Length];
            for (int i = 0; i < globalPath.poses.Length; i++)
            {
                //机器人世界坐标
                float x = globalPath.poses[i].pose.position.x;
                float y = globalPath.poses[i].pose.position.y;
                //世界坐标系转视图坐标系
                System.Windows.Point point = RosHelper.WorldToView(new System.Windows.Point(x, y));
                pointFs[i] = new PointF((float)point.X, (float)point.Y);
            }
            if (globalPath.poses.Length > 0)
            {
                //绘制机路径
                g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
                g.DrawLines(new Pen(Color.YellowGreen, 2f), pointFs);
                //Bitmap转ImageSource
                this.Dispatcher.Invoke(new Action(delegate
                {
                    System.Windows.Controls.Image image = new System.Windows.Controls.Image();
                    image.Source = MapHlper.ChangeBitmapToImageSource(im);
                    image.Margin = new Thickness(0, 0, 0, 0);

                    cvGlobalPlan.Children.Clear();
                    cvGlobalPlan.Children.Add(image);
                }));
            }
        }
        //绘制局部路径
        private void DrawgLocalPlan()
        {
            if (localPath == null || localPath.poses.Length < 2)
                return;
            int width = RosHelper.viewSize.Width;
            int height = RosHelper.viewSize.Height;
            Bitmap im = new Bitmap(width, height);
            Graphics g = Graphics.FromImage(im);
            //g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;
            PointF[] pointFs = new PointF[localPath.poses.Length];
            for (int i = 0; i < localPath.poses.Length; i++)
            {
                //机器人世界坐标
                float x = localPath.poses[i].pose.position.x;
                float y = localPath.poses[i].pose.position.y;
                //世界坐标系转视图坐标系
                System.Windows.Point point = RosHelper.WorldToView(new System.Windows.Point(x, y));
                pointFs[i] = new PointF((float)point.X, (float)point.Y);
            }
            //绘制机路径
            g.DrawLines(new Pen(Color.Blue), pointFs);
            //Bitmap转ImageSource
            this.Dispatcher.Invoke(new Action(delegate
            {
                System.Windows.Controls.Image image = new System.Windows.Controls.Image();
                image.Source = MapHlper.ChangeBitmapToImageSource(im);
                image.Margin = new Thickness(0, 0, 0, 0);

                cvLocalPlan.Children.Clear();
                cvLocalPlan.Children.Add(image);
            }));
        }
        /// <summary>
        /// 绘制位置指示器
        /// </summary>
        /// <param name="point">原点坐标</param>
        /// <param name="angle">指示器角度</param>
        private void DrawPostionCursor(System.Windows.Point point, float angle = 0)
        {
            int radio = 24;
            float distance = radio + 30;

            //中心坐标
            System.Windows.Point pointCenter = point;
            //直线末端坐标
            System.Windows.Point pointEnd = new System.Windows.Point();
            pointEnd.X = (int)(point.X + (float)(Math.Cos(Math.PI * (angle / 180.0)) * distance));
            pointEnd.Y = (int)(point.Y - (float)(Math.Sin(Math.PI * (angle / 180.0)) * distance));

            cvPostion.Children.Clear();
            //长方形
            Line myLine = new Line();
            myLine.X1 = pointCenter.X;
            myLine.X2 = pointEnd.X;
            myLine.Y1 = pointCenter.Y;
            myLine.Y2 = pointEnd.Y;
            myLine.StrokeThickness = 5;
            myLine.Stroke = System.Windows.Media.Brushes.LightSteelBlue;
            myLine.Fill = System.Windows.Media.Brushes.LightSteelBlue;
            cvPostion.Children.Add(myLine);

            //圆
            var el = new Ellipse();
            el.Height = radio * 2;
            el.Width = radio * 2;
            el.Margin = new Thickness(pointCenter.X - radio, pointCenter.Y - radio, 0, 0);
            el.StrokeThickness = 2;
            el.Fill = System.Windows.Media.Brushes.GreenYellow;
            el.Stroke = System.Windows.Media.Brushes.Gray;
            cvPostion.Children.Add(el);
        }
        private void ClearPostionCursor()
        {
            cvPostion.Children.Clear();
        }
        //整条路径显示
        private void DrawRoute()
        {
            int radio = (int)(6 * RosHelper.viewScale);
            float distance = radio;
            cvRoute.Children.Clear();
            System.Windows.Point Lastpoint = new System.Windows.Point();
            //画虚线
            for (int i = 0; i < GlobalVar.routeInfo.StationTotal; i++)
            {
                System.Windows.Point point = new System.Windows.Point();
                point.X = GlobalVar.routeInfo.stations[i].Pose.position.x;
                point.Y = GlobalVar.routeInfo.stations[i].Pose.position.y;
                //世界坐标转视图坐标
                point = RosHelper.WorldToView(point);
                //中心坐标
                System.Windows.Point pointCenter = point;
                //角度
                var angle = GlobalVar.routeInfo.stations[i].StationInfoAngel;

                //直线末端坐标
                System.Windows.Point pointEnd = new System.Windows.Point();
                pointEnd.X = (int)(point.X + (float)(Math.Cos(Math.PI * (angle / 180.0)) * distance));
                pointEnd.Y = (int)(point.Y - (float)(Math.Sin(Math.PI * (angle / 180.0)) * distance));

                //将站点用线连接起来
                if (i > 0)//第一个点不画线
                {
                    Line l = new Line();
                    l.X1 = Lastpoint.X;
                    l.X2 = pointCenter.X;
                    l.Y1 = Lastpoint.Y;
                    l.Y2 = pointCenter.Y;
                    l.Stroke = System.Windows.Media.Brushes.Coral;
                    l.StrokeThickness = 2 * RosHelper.viewScale;
                    l.StrokeDashArray = new System.Windows.Media.DoubleCollection() { 2, 3 };
                    l.StrokeDashCap = System.Windows.Media.PenLineCap.Triangle;
                    l.StrokeEndLineCap = System.Windows.Media.PenLineCap.Square;
                    l.StrokeStartLineCap = System.Windows.Media.PenLineCap.Round;
                    cvRoute.Children.Add(l);
                }
                Lastpoint = point;
            }
            //显示所有站点
            for (int i = 0; i < GlobalVar.routeInfo.StationTotal; i++)
            {
                System.Windows.Point point = new System.Windows.Point();
                point.X = GlobalVar.routeInfo.stations[i].Pose.position.x;
                point.Y = GlobalVar.routeInfo.stations[i].Pose.position.y;
                //世界坐标转视图坐标
                point = RosHelper.WorldToView(point);
                //中心坐标
                System.Windows.Point pointCenter = point;
                //角度
                var angle = GlobalVar.routeInfo.stations[i].StationInfoAngel;

                //直线末端坐标
                System.Windows.Point pointEnd = new System.Windows.Point();
                pointEnd.X = (int)(point.X + (float)(Math.Cos(Math.PI * (angle / 180.0)) * distance));
                pointEnd.Y = (int)(point.Y - (float)(Math.Sin(Math.PI * (angle / 180.0)) * distance));

                //圆
                var el = new Ellipse();
                el.Height = radio * 2;
                el.Width = radio * 2;
                el.Margin = new Thickness(pointCenter.X - radio, pointCenter.Y - radio, 0, 0);
                el.StrokeThickness = 2;
                if (GlobalVar.systemConfig.RouteConfig.StationNow == i)
                    el.Fill = System.Windows.Media.Brushes.Green;
                else
                    el.Fill = System.Windows.Media.Brushes.Orange;
                el.Stroke = System.Windows.Media.Brushes.Gray;
                cvRoute.Children.Add(el);

                //方向线
                Line line = new Line();
                line.X1 = pointCenter.X;
                line.X2 = pointEnd.X;
                line.Y1 = pointCenter.Y;
                line.Y2 = pointEnd.Y;
                line.StrokeThickness = 1f * RosHelper.viewScale;
                if (GlobalVar.systemConfig.RouteConfig.StationNow == i)
                    line.Stroke = System.Windows.Media.Brushes.Black;
                else
                    line.Stroke = System.Windows.Media.Brushes.Red;
                cvRoute.Children.Add(line);

                //编号
                System.Windows.Controls.Label label = new System.Windows.Controls.Label();
                label.Margin = new Thickness(pointCenter.X - radio, pointCenter.Y - radio, 0, 0);
                label.Content = string.Format("S{0}", i);
                label.FontSize = 22;
                label.Foreground = System.Windows.Media.Brushes.Black;

                cvRoute.Children.Add(label);
            }

        }

        //更新所有显示
        private void UpdateAllCv()
        {
            DrawRobot();
            DrawgLocalPlan();
            DrawMap();
            DrawGlobalPlan();
            //显示路径
            DrawRoute();
        }
        #endregion

        #region 控制机器人按钮
        private void btnGo_Click(object sender, RoutedEventArgs e)
        {
            string sub;
            GlobalVar.TocpicList.TryGetValue("cmd_vel", out sub);
            GeometryTwist message = new GeometryTwist();
            message.linear.x = 0.15f;
            GlobalVar.rosSocket.Publish(sub, message);
        }

        private void btnBack_Click(object sender, RoutedEventArgs e)
        {
            string sub;
            GlobalVar.TocpicList.TryGetValue("cmd_vel", out sub);
            GeometryTwist message = new GeometryTwist();
            message.linear.x = -0.15f;
            GlobalVar.rosSocket.Publish(sub, message);
        }

        private void btnLeft_Click(object sender, RoutedEventArgs e)
        {
            string sub;
            GlobalVar.TocpicList.TryGetValue("cmd_vel", out sub);
            GeometryTwist message = new GeometryTwist();
            message.angular.z = 0.5f;
            GlobalVar.rosSocket.Publish(sub, message);
        }

        private void btnRight_Click(object sender, RoutedEventArgs e)
        {
            string sub;
            GlobalVar.TocpicList.TryGetValue("cmd_vel", out sub);
            GeometryTwist message = new GeometryTwist();
            message.angular.z = -0.5f;
            GlobalVar.rosSocket.Publish(sub, message);
        }

        private void btnStop_Click(object sender, RoutedEventArgs e)
        {
            string sub;
            GlobalVar.TocpicList.TryGetValue("cmd_vel", out sub);
            GeometryTwist message = new GeometryTwist();
            GlobalVar.rosSocket.Publish(sub, message);
        }
        #endregion

        #region 鼠标事件
        //滚轮缩放
        private void image_PreviewMouseWheel(object sender, System.Windows.Input.MouseWheelEventArgs e)
        {

            float offset = 0;
            if (e.Delta > 0)
                offset = 0.1f;
            else
                offset = -0.1f;
            RosHelper.viewScale += offset;
            if (RosHelper.viewScale < 0)
            {
                RosHelper.viewScale = 0;
            }
            //四射五入，保留一位小数
            RosHelper.viewScale = (float)Math.Round(RosHelper.viewScale, 1);
            bottomInfo.ViewScale = string.Format("视图缩放: {0}", RosHelper.viewScale);

            UpdateAllCv();
        }
        //右键按下
        private void imageRobot_PreviewMouseRightButtonDown(object sender, System.Windows.Input.MouseButtonEventArgs e)
        {
            //记录鼠标按下时的位置
            GlobalVar.mouseRightBtnDownPoint = e.GetPosition(gridDraw);
            //获取当前坐标
            System.Windows.Point nowPoint = e.GetPosition(gridDraw);
            //编辑路径
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.RouteEdit)
            {
                //站点世界坐标转视图坐标
                float x = GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.x;
                float y = GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.y;

                System.Windows.Point point = RosHelper.WorldToView(new System.Windows.Point(x, y));
                //计算角度
                double angle = RosHelper.PointToAngle(RosHelper.ViewToAxisPoint(point), RosHelper.ViewToAxisPoint(nowPoint));
                GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].StationInfoAngel = angle;
                GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.orientation.z = (float)Math.Sin(angle / 2.0 * Math.PI / 180.0);
                GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.orientation.w = (float)Math.Cos(angle / 2.0 * Math.PI / 180.0);

                //更新站点图标
                DrawRoute();
                //更新界面
                RouteInfoUpdate();
            }
            else
            {
                GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.MoveView;
                this.Cursor = Cursors.Hand;
            }
        }
        //右键抬起
        private void imageRobot_PreviewMouseRightButtonUp(object sender, MouseButtonEventArgs e)
        {
            if (GlobalVar.mouseFunction != GlobalVar.MouseFunctionEnum.RouteEdit)
            {
                GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.None;
                this.Cursor = Cursors.Arrow;
            }
        }
        //左键按下
        private void imageRobot_PreviewMouseLeftButtonDown(object sender, MouseButtonEventArgs e)
        {
            //记录按下时的位置
            GlobalVar.mouseLeftBtnDownPoint = e.GetPosition(gridDraw);
            //初始化机器人位置
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.ResetRobot)
            {
                //绘制位置指示器
                DrawPostionCursor(GlobalVar.mouseLeftBtnDownPoint);
            }
            else
            //手动设置机器人目标位置
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.SetTargetPose)
            {
                //绘制位置指示器
                DrawPostionCursor(GlobalVar.mouseLeftBtnDownPoint);
            }
            else
            //站点编辑
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.RouteEdit)
            {
                PointF pointf = RosHelper.ViewToWorld(GlobalVar.mouseLeftBtnDownPoint);

                GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.x = pointf.X;
                GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.y = pointf.Y;
                //更新站点图标
                DrawRoute();
                //更新界面
                RouteInfoUpdate();
            }
        }
        //左键抬起
        private void imageRobot_PreviewMouseLeftButtonUp(object sender, MouseButtonEventArgs e)
        {
            //发布机器人目标位置
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.SetTargetPose)
            {
                //清除标志
                GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.None;
                //清除指示器
                ClearPostionCursor();
                //界面坐标系转真实坐标系
                PointF pointf = RosHelper.ViewToAxis(GlobalVar.mouseLeftBtnDownPoint);
                //真实坐标系转世界坐标系
                pointf = RosHelper.AxisToWorld(pointf, GlobalVar.mapGrid);

                //获取当前坐标
                System.Windows.Point nowPoint = e.GetPosition(gridDraw);
                //计算角度
                double angle = RosHelper.PointToAngle(RosHelper.ViewToAxisPoint(GlobalVar.mouseLeftBtnDownPoint), RosHelper.ViewToAxisPoint(nowPoint));

                //发布目标位置话题
                string publication__id_Go = GlobalVar.rosSocket.Advertise("/move_base_simple/goal", MessageList.geometry_msgs.PoseStamped);

                GeometryPoseStamped gps = new GeometryPoseStamped();
                gps.header.frame_id = "map";
                gps.pose.position.x = pointf.X;
                gps.pose.position.y = pointf.Y;

                gps.pose.orientation.z = (float)Math.Sin(angle / 2.0 * Math.PI / 180.0);
                gps.pose.orientation.w = (float)Math.Cos(angle / 2.0 * Math.PI / 180.0);
                GlobalVar.rosSocket.Publish(publication__id_Go, gps);

                msg(string.Format(
                    "发布了一个坐标:\r\n" +
                    "X:{0}\r\n " +
                    "Y:{1}\r\n " +
                    "Z:{2}\r\n " +
                    "W:{3}",
                    gps.pose.position.x,
                    gps.pose.position.y,
                    gps.pose.orientation.z,
                    gps.pose.orientation.w
                    ), System.Windows.Media.Brushes.Orange);
            }
            else
            //重置机器人位置
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.ResetRobot)
            {
                //清除标志
                GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.None;
                //清除指示器
                ClearPostionCursor();
                //界面坐标系转真实坐标系
                PointF pointf = RosHelper.ViewToAxis(GlobalVar.mouseLeftBtnDownPoint);
                //真实坐标系转世界坐标系
                pointf = RosHelper.AxisToWorld(pointf, GlobalVar.mapGrid);

                //获取当前坐标
                System.Windows.Point nowPoint = e.GetPosition(gridDraw);
                //计算角度
                double angle = RosHelper.PointToAngle(RosHelper.ViewToAxisPoint(GlobalVar.mouseLeftBtnDownPoint), RosHelper.ViewToAxisPoint(nowPoint));

                //发布目标位置话题
                string resetId = GlobalVar.rosSocket.Advertise("/initialpose", MessageList.geometry_msgs.PoseWithCovarianceStamped);

                GeometryPoseWithCovarianceStamped gps = new GeometryPoseWithCovarianceStamped();
                gps.header.frame_id = "map";
                gps.pose.pose.position.x = pointf.X;
                gps.pose.pose.position.y = pointf.Y;
                gps.pose.pose.orientation.z = (float)Math.Sin(angle / 2.0 * Math.PI / 180.0);
                gps.pose.pose.orientation.w = (float)Math.Cos(angle / 2.0 * Math.PI / 180.0);
                GlobalVar.rosSocket.Publish(resetId, gps);

                msg(string.Format(
                    "重置机器人坐标:\r\n" +
                    "X:{0}\r\n " +
                    "Y:{1}\r\n " +
                    "Z:{2}\r\n " +
                    "W:{3}",
                    gps.pose.pose.position.x,
                    gps.pose.pose.position.y,
                    gps.pose.pose.orientation.z,
                    gps.pose.pose.orientation.w
                    ), System.Windows.Media.Brushes.Yellow);
            }
        }
        //鼠标移动
        private void imageRobot_PreviewMouseMove(object sender, System.Windows.Input.MouseEventArgs e)
        {
            //获取当前坐标
            System.Windows.Point nowPoint = e.GetPosition(gridDraw);

            //更新底部坐标信息
            bottomInfo.ViewAxis = string.Format("视图坐标: X:{0}, Y:{1}", (int)nowPoint.X, (int)nowPoint.Y);

            //发布位置
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.SetTargetPose)
            {
                if (e.LeftButton == MouseButtonState.Pressed)
                {
                    //计算角度
                    double angle = RosHelper.PointToAngle(RosHelper.ViewToAxisPoint(GlobalVar.mouseLeftBtnDownPoint), RosHelper.ViewToAxisPoint(nowPoint));
                    //绘制箭头
                    DrawPostionCursor(GlobalVar.mouseLeftBtnDownPoint, (int)angle);
                }
            }
            else
            //移动视图
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.MoveView)
            {
                if (e.RightButton == MouseButtonState.Pressed)
                {
                    //比较移动的像素
                    RosHelper.originPoint.X += (int)(nowPoint.X - GlobalVar.mouseRightBtnDownPoint.X);
                    RosHelper.originPoint.Y += (int)(nowPoint.Y - GlobalVar.mouseRightBtnDownPoint.Y);
                    bottomInfo.OriginPoint = string.Format("原点坐标：X:{0},Y:{1}", RosHelper.originPoint.X, RosHelper.originPoint.Y);

                    UpdateAllCv();

                    GlobalVar.mouseRightBtnDownPoint = nowPoint;
                }
            }
            else
            //重置机器人位置
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.ResetRobot)
            {
                if (e.LeftButton == MouseButtonState.Pressed)
                {
                    //计算角度
                    double angle = RosHelper.PointToAngle(RosHelper.ViewToAxisPoint(GlobalVar.mouseLeftBtnDownPoint), RosHelper.ViewToAxisPoint(nowPoint));
                    //绘制箭头
                    DrawPostionCursor(GlobalVar.mouseLeftBtnDownPoint, (int)angle);
                }
            }
            else
            //编辑路径
            if (GlobalVar.mouseFunction == GlobalVar.MouseFunctionEnum.RouteEdit)
            {
                //左键移动位置
                if (e.LeftButton == MouseButtonState.Pressed)
                {
                    PointF pointf = RosHelper.ViewToWorld(nowPoint);

                    GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.x = pointf.X;
                    GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.y = pointf.Y;
                    //更新站点图标
                    // DrawStationNow();
                    DrawRoute();
                    //更新界面
                    RouteInfoUpdate();
                }
                else
                //右键调整角度
                if (e.RightButton == MouseButtonState.Pressed)
                {
                    //站点世界坐标转视图坐标
                    float x = GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.x;
                    float y = GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.position.y;

                    System.Windows.Point point = RosHelper.WorldToView(new System.Windows.Point(x, y));
                    //计算角度
                    double angle = RosHelper.PointToAngle(RosHelper.ViewToAxisPoint(point), RosHelper.ViewToAxisPoint(nowPoint));
                    GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].StationInfoAngel = angle;
                    GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.orientation.z = (float)Math.Sin(angle / 2.0 * Math.PI / 180.0);
                    GlobalVar.routeInfo.stations[GlobalVar.systemConfig.RouteConfig.StationNow].Pose.orientation.w = (float)Math.Cos(angle / 2.0 * Math.PI / 180.0);

                    //更新站点图标
                    DrawRoute();
                    //更新界面
                    RouteInfoUpdate();
                }
            }
        }
        #endregion

        #region 路径编辑
        private void MenuItem_AddRoute_Click(object sender, RoutedEventArgs e)
        {
            //弹出新增路径对话框
            AddRouteWindow window = new AddRouteWindow();
            window.ShowDialog();
            //更新当前路径
            //更新路径编辑界面
            RouteEditInit();
        }
        //下一个
        private void btnNextStation_Click(object sender, RoutedEventArgs e)
        {
            int i = GlobalVar.systemConfig.RouteConfig.StationNow;
            i++;
            if (i >= GlobalVar.routeInfo.StationTotal)
                i = 0;
            GlobalVar.systemConfig.RouteConfig.StationNow = i;
            //更新界面
            DrawRoute();
            //更新信息
            RouteInfoUpdate();
        }
        //上一个
        private void btnLastStation_Click(object sender, RoutedEventArgs e)
        {
            int i = GlobalVar.systemConfig.RouteConfig.StationNow;
            i--;
            if (i < 0)
                i = GlobalVar.routeInfo.StationTotal - 1;
            GlobalVar.systemConfig.RouteConfig.StationNow = i;
            //更新界面
            DrawRoute();
            //更新信息
            RouteInfoUpdate();
        }
        //新增
        private void btnAddStation_Click(object sender, RoutedEventArgs e)
        {
            btnEditStation.Content = "完  成";
            GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.RouteEdit;

            //添加站点
            Station station = new Station();
            //如果有机器人当前位置，将当前位置设置为默认
            if (AmclPoseStamped != null)
            {
                station.Pose = AmclPoseStamped.pose.pose;
                //计算机器人角度
                station.StationInfoAngel = RosHelper.QuaternionZToAngle(AmclPoseStamped.pose.pose.orientation.z);
            }

            GlobalVar.routeInfo.stations.Add(station);

            //更新全局当前站点编号
            GlobalVar.systemConfig.RouteConfig.StationNow = GlobalVar.routeInfo.stations.Count - 1;
            //更新站点总数
            GlobalVar.routeInfo.StationTotal += 1;

            //更新站点图标
            DrawRoute();
            //更新界面
            RouteInfoUpdate();
        }
        //运行
        Thread threadRunStaion;
        private void btnRunStation_Click(object sender, RoutedEventArgs e)
        {
            if (threadRunStaion.ThreadState == ThreadState.Running)
            {
                threadRunStaion.Abort();
                btnRunStation.Content = "运行";
            }
            else
            {
                threadRunStaion = new Thread(autoRunThread);
                threadRunStaion.IsBackground = true;
                threadRunStaion.Start();
                btnRunStation.Content = "取消";
            }
        }

        private void autoRunThread()
        {
            int seq = moveBaseActionResult.header.seq;
            for (int i = 0; i < GlobalVar.routeInfo.StationTotal; i++)
            {
                //发布目标位置话题
                string publication__id_Go = GlobalVar.rosSocket.Advertise("/move_base_simple/goal", MessageList.geometry_msgs.PoseStamped);

                GeometryPoseStamped gps = new GeometryPoseStamped();
                gps.header.frame_id = "map";
                gps.pose = GlobalVar.routeInfo.stations[i].Pose;

                GlobalVar.rosSocket.Publish(publication__id_Go, gps);
                this.Dispatcher.Invoke(new Action(delegate
                {
                msg(string.Format(
                    "发布第{4}个坐标:\r\n" +
                    "X:{0}\r\n " +
                    "Y:{1}\r\n " +
                    "Z:{2}\r\n " +
                    "W:{3}",
                    gps.pose.position.x,
                    gps.pose.position.y,
                    gps.pose.orientation.z,
                    gps.pose.orientation.w,
                        i
                        ), System.Windows.Media.Brushes.Orange);
                }));


                //等带到达
                while (moveBaseActionResult.header.seq == seq)
                {
                    Thread.Sleep(10);
                }
                seq = moveBaseActionResult.header.seq;
            }
        }

        //编辑
        private void btnEditStation_Click(object sender, RoutedEventArgs e)
        {
            if (btnEditStation.Content.ToString() == "完  成")
            {
                btnEditStation.Content = "编  辑";
                GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.None;
                SaveRoute();
                SaveConfig();
            }
            else
            {
                btnEditStation.Content = "完  成";
                GlobalVar.mouseFunction = GlobalVar.MouseFunctionEnum.RouteEdit;
            }
        }
        //删除路径
        private void btnDeleteStation_Click(object sender, RoutedEventArgs e)
        {
            if (GlobalVar.routeInfo.StationTotal == 0)
            {
                MessageBox.Show("没有站点可以删了！");
                return;
            }
            MessageBoxResult dr = MessageBox.Show("确定要删除吗?", "提示", MessageBoxButton.OKCancel, MessageBoxImage.Question);
            if (dr == MessageBoxResult.OK)
            {
                if (GlobalVar.routeInfo.stations != null)
                {
                    //删除当前站点
                    GlobalVar.routeInfo.stations.RemoveAt(GlobalVar.systemConfig.RouteConfig.StationNow);
                    //更新系统配置
                    GlobalVar.routeInfo.StationTotal -= 1;
                    if (GlobalVar.routeInfo.StationTotal == 0)
                        GlobalVar.systemConfig.RouteConfig.StationNow = 0;
                    else
                    if (GlobalVar.systemConfig.RouteConfig.StationNow == GlobalVar.routeInfo.StationTotal)
                        GlobalVar.systemConfig.RouteConfig.StationNow -= 1;

                    SaveRoute();
                    SaveConfig();
                    RouteInfoUpdate();
                    DrawRoute();
                }
            }
        }
        //选择路径
        private void labelRouteNow_MouseDoubleClick(object sender, MouseButtonEventArgs e)
        {
            //弹出选择路径
            SelectRouteWindow window = new SelectRouteWindow();
            window.ShowDialog();
            //更新路径编辑界面
            RouteEditInit();
            //更新绘图界面
            DrawRoute();
        }
        #endregion

        private void SaveRoute()
        {
            //保存路径
            SqliteHelper sql = new SqliteHelper();
            //存储数据
            string str = JsonConvert.SerializeObject(GlobalVar.routeInfo, Formatting.Indented);
            //存储
            int res = sql.ExecuteNonQuery(string.Format("update route set route='{0}' where num={1}", str, GlobalVar.routeInfo.RouteNum));
            if (res <= 0)
            {
                msg("路径更改存储失败！", System.Windows.Media.Brushes.Red);
            }
            else
                msg("路径更改存储成功!");
        }
        private void SaveConfig()
        {
            //保存系统参数
            SqliteHelper sql = new SqliteHelper();
            //存储数据
            string str = JsonConvert.SerializeObject(GlobalVar.systemConfig, Formatting.Indented);
            //存储
            int res = sql.ExecuteNonQuery(string.Format("update config set value='{0}' where key='{1}'", str, "system"));
            if (res <= 0)
            {
                msg("系统配置存储失败！", System.Windows.Media.Brushes.Red);
            }
            else
                msg("系统配置存储成功!");
        }
        //重置窗口
        private void RestView()
        {
            //计算地图与视窗的长宽比例
            float wScale = RosHelper.viewSize.Width / (float)RosHelper.validArea.Width;
            float hScale = RosHelper.viewSize.Height / (float)RosHelper.validArea.Height;
            //选择缩放比例较小的一个
            RosHelper.viewScale = (wScale < hScale) ? wScale : hScale;
            //计算中心点
            float wNow = (RosHelper.viewSize.Width - RosHelper.validArea.Width * RosHelper.viewScale * 2) / 2;
            float hNow = (RosHelper.viewSize.Height - (RosHelper.viewSize.Height - RosHelper.validArea.Height) * RosHelper.viewScale * 2 + RosHelper.validArea.Height / 4 * RosHelper.viewScale) / 2;
            //移动视图原点
            RosHelper.originPoint = new System.Drawing.Point((int)wNow, (int)hNow);
            UpdateBottomInfo();
            UpdateAllCv();
        }

        //窗口尺寸变化事件
        private void Window_SizeChanged(object sender, SizeChangedEventArgs e)
        {
            if (this.IsLoaded)
            {
                //绘图区尺寸
                RosHelper.viewSize.Width = (int)gridDraw.ActualWidth;
                RosHelper.viewSize.Height = (int)gridDraw.ActualHeight;
                RestView();
            }
        }
    }
}
